package com.xsoft.navigation.pdr;

import android.location.Location;

/**
 * created by zhangkai on 2021/8/2
 * email: 405547628@qq.com
 * desc:
 */
public class StepPositioningHandler {
    private Location currentLocation;
    private static final int eRadius = 6371000; //rayon de la terre en m

    public Location getCurrentLocation() {
        return currentLocation;
    }

    public void setCurrentLocation(Location mCurrentLocation) {
        this.currentLocation = mCurrentLocation;
    }

    /**
     * Calcule la nouvelle position de l'utilisateur  partir de la courante
     *
     * @param stepSize la taille du pas qu'a fait l'utilisateur
     * @param bearing  l'angle de direction
     * @return la nouvelle localisation
     */
    public Location computeNextStep(float stepSize, float bearing) {
        Location newLoc = new Location(currentLocation);
        float angDistance = stepSize / eRadius;
        double oldLat = currentLocation.getLatitude();
        double oldLng = currentLocation.getLongitude();
        double newLat = Math.asin(Math.sin(Math.toRadians(oldLat)) * Math.cos(angDistance) +
                Math.cos(Math.toRadians(oldLat)) * Math.sin(angDistance) * Math.cos(bearing));
        double newLon = Math.toRadians(oldLng) +
                Math.atan2(Math.sin(bearing) * Math.sin(angDistance) * Math.cos(Math.toRadians(oldLat)),
                        Math.cos(angDistance) - Math.sin(Math.toRadians(oldLat)) * Math.sin(newLat));
        //reconversion en degres
        newLoc.setLatitude(Math.toDegrees(newLat));
        newLoc.setLongitude(Math.toDegrees(newLon));

        newLoc.setBearing((currentLocation.getBearing() + 180) % 360);
        currentLocation = newLoc;

        return newLoc;
    }
}
